#pragma once
// #include "zuc/nml_intf/zucpos.h"
// #include "kinematics.h"
// #include "motion.h"
#include "stdbool.h"
// #include "errcode.h"
#include "zuc/nml_intf/zucpos2.h"
#include "tp_jkzuc_data_type.h"
#include <memory>
// #include "tp_unique_data_type.hh"

//接口类
typedef struct TP_STRUCT_ TP_STRUCT;

namespace kine
{
    class KineItf;
};

class JAKADynIntf;
typedef struct safetyConstraint
{
    double stoppingDist;                //停止距离  
    double stoppingTime;                //停止时间
    double tcpVelLimit;                 //TCP速度限制
    double stoppingDistReduce;          //缩减模式停止距离
    double stoppingTimeReduce;          //缩减模式停止时间
    double tcpVelLimitReduce;           //缩减模式tcp速度限制
    double elbowVelLimitReduce;         //缩减模式肘部速度限制
    int enableLimitJointVel;            //关节速度限制开关
} safetyConstraint;

typedef struct BlendsConfig
{
    int arcBlendEnable;
    int arcBlendFallbackEnable;
    int arcBlendOptDepth;
    int arcBlendGapCycles;
    double arcBlendRampFreq;
    double arcBlendTangentKinkRatio;
    int enableMovejMovelBlend = 1;
    int enableMovejMovejBlend = 1;
} BlendsConfig;
namespace TP
{
typedef struct TP_Config_Stable
{
    int _queueSize;         //Tc队列大小，目前宏定义在控制器中，应该挪到这里吧？
    double cycleTime;       //控制器周期  TP周期
    int joint_num;          //关节轴数
    double ini_vel_trans;   //机型配置文件中笛卡尔最大速度
    double ini_vel_ori;     //机型配置文件中姿态最大速度
    double ini_acc_trans;   //机型配置文件中笛卡尔最大加速度
    double ini_acc_ori;     //机型配置文件中姿态最大加速度
    int robot_serial_num;
}TP_Config_Stable;

typedef struct TP_Status_In   //这里存的是每个周期要更新的状态  以及及机器人状态信息  
{
    ZucPose tool_offset;                                        //tool
    PmRpy base_offset;                                          //base
    ZucPose user_offset;                                        //user        
    int max_fw_len = 40;                                         //MAX_FW_LEN 
    int max_planning_queue_size = 80;                                //TOPP.MAX_FW_LEN
    double maxFeedScale = 1.0;                                        //zucmotConfig->maxFeedScale  代码中写死的1.0
    PmHomogeneous o_to_world;                                   //传送带 zucmotCommand->enaconvyrtrack.o_to_world  圆心在世界坐标系的表示吧
    int percentage_mode_level;                                  //保护性停止等级 zucmotCommand.mot_percentage_mode.level 
    double net_feed_scale;                                      // zucmotStatus->net_feed_scale zucmotStatus->rapid_scale 多处调用并赋值
    double momvelscale = 1.0;                                   //原仓库中恒等于1  只在SaftyAction中被更改 但是被注释掉了
    unsigned char enables_new;                                  //在command中 ZUCMOT_AF_ENABLE 被位操作修改 可直接改成运动至该点为1 其余为0
    int reduce_mode;                                            //是否触发缩减
    int still_dec;                                              //传送带
    ZucPose disable_convyr_real_pos;                            //传送带
    double actualTorque[ZUCMOT_MAX_JOINTS];                     //力控相关 实际的力矩 zucmotStatus->actualTorque
    double Acclimit_axes[ZUCMOT_MAX_AXIS];                      //USERSETTINGINI中笛卡尔关节最大加速度限制
    double Vellimit_axes[ZUCMOT_MAX_AXIS];                      //USERSETTINGINI中笛卡尔关节最大速度限制
    int stepping;
    double vellimit_joint[ZUCMOT_MAX_JOINTS];                   //USERSETTINGINI中笛卡尔关节最大加速度限制
    double acclimit_joint[ZUCMOT_MAX_JOINTS];                   //USERSETTINGINI中笛卡尔关节最大加速度限制
    double j_pos_limit_up[ZUCMOT_MAX_JOINTS];                   //关节位置上限
    double j_pos_limit_low[ZUCMOT_MAX_JOINTS];                  //关节位置下限
    double joint_pos_fb[ZUCMOT_MAX_JOINTS];                     //反馈位置更新
    double rated_torq[ZUCMOT_MAX_JOINTS];                        //机型文件中 MOTOR_RATED_TORQ
    double joint_pos_cmd[ZUCMOT_MAX_JOINTS];                    //碰撞反弹会用到  关节指令位置，现在应该跟last_pos_cmds是一个值
    double joint_veldir[ZUCMOT_MAX_JOINTS];
    double rated_trans_ratio[ZUCMOT_MAX_JOINTS];                //机型文件中的减速比 MOTOR_RATED_TRANS_RATIO
    int IO_value[ZUCMOT_MAX_DIO];
    int DO_value;
    int AO_value;
    // double tcpVelLimit;
    int safezone_Enable = 0;                                        //安全平面开关
    int safezone_Dengrous = 0;
    double elbow_sphere_radius;                                 //肘部球半径 机型文件中SAFEZONE_ELBOW_RADIUS
    int check_safezone_prediction;
    // double trajCycleTime;
    ZucPose carte_pos_cmd;                                      //碰撞反弹会用到  笛卡尔指令位置，现在应该跟last_pos_cmds是一个值
    ZucPose carte_pos_fb;
    int inv_flag_7_dof;                                         //七轴逆接模式
    int threePositionEnableLimit = 0;                           //三位置使能
    int tcpForceLimit = 0;  
    int is_unlocked[ZUCMOT_MAX_JOINTS];
    int is_track_pre_singular = 0;                              //传送带
    
    RobJointVal last_pos_cmds;     //規畫前指令關節位置
    RobJointVal q_fb;              //规划前反馈关节角
    safetyConstraint safetyCons;   //安全功能参数
    BlendsConfig arc_blends_param;   //圆弧过度参数


}TP_Status_In;

typedef struct TP_Status_Out
{
    FtConfig ftConfig[ZUCMOT_MAX_JOINTS];
    int unlock[ZUCMOT_MAX_JOINTS];
    int is_jointvel_limited[ZUCMOT_MAX_JOINTS];     //是否被关节速度限制输出
    ZucPose carte_pos_cmd;
    ZucPose carte_pos_desired;                      //当前周期输出笛卡尔期望位置
    double jpos_cmd[ZUCMOT_MAX_JOINTS];
    int max_distance_stop_flag;                     
    int motion_type;                                //当前插补周期的运动段类型
    int parabolic_blending;                         //当前周期为pb转接
    int is_tcpvel_limited;                          //是否被TCP速度限制输出
    int is_interpolation_flag;                      //是否插补成功标志位
    double currentPos_joint[ZUCMOT_MAX_JOINTS];     //下一周期期望关节位置
    int enables;                                     //是否为关节、直线运动至该点 
    double current_vel;                             //当前规划的指令速度、与规划主轴单位一致 T规划的本周期速度 对应主仓库zucmotStatus->current_vel
    double distance_to_go;                          //当前轨迹剩余主轴长度

}TP_Status_Out;

typedef struct TP_Status   //这里存TP的状态
{
    double velScale;
    double trq_limit[ZUCMOT_MAX_JOINTS];
}TP_Status;



// 接口类
class TP_Itf
{
public:
    int curTpID;
    TP_STRUCT* coord_tp;

    std::shared_ptr<kine::KineItf> kine_;
    std::shared_ptr<::JAKADynIntf> dyna_;
    TP_Config_Stable config_;
    TP_Status_In status_in_;
    TP_Status_Out status_out_;
    TP_Status status_;
    // void tpManager_init() = 0;
    virtual int tpManger_GetCurTpId() = 0;

    virtual int init(TP_Config_Stable tp_config, TP_Status_In status_jkzuc_in, std::shared_ptr<kine::KineItf> kine, std::shared_ptr<::JAKADynIntf> dyna) = 0;
        
    virtual int update_tp_status(TP_Status_In status_jkzuc_in) = 0;

    virtual int tpManager_tppause() = 0;

    virtual int tpManager_tpresume() = 0;

    virtual void tpManager_tpabort() = 0;
    virtual int tpManager_tpErrorID() = 0;
    virtual void tpManager_tpclear() = 0;
    virtual void tpManager_tpDisableTrack() = 0;
    
    virtual void tpManager_SetCollisionOption(CollisionOption* param) = 0;

    // ============= motion status ===

    virtual int tpManager_tpCollision_rebound() = 0;
    virtual int tpManager_get_rebound_pos() = 0;
    virtual int tpManager_tpIsDone() = 0;

    virtual int tpManager_tpIsAbort() = 0;
    virtual int tpManager_tpIsPausing() = 0;
    virtual int tpManager_tpSych_singular_dist() = 0;
    virtual void tpManager_tpResetSych_singular_dist() = 0;
    virtual double tpManager_GetDesiredVel() = 0;
    virtual void tpManager_GetcurSegTargetPose(ZucPose* const pos) = 0;

    // ==================== tc status
    virtual int tpManager_GetMotId(int index) = 0;
    virtual double tpManager_GetMotProgress(int index) = 0;
    virtual double tpManager_GetMotEStopProgress(int index) = 0;
    virtual void tpManager_SetMotProgress(int index, double progess) = 0;
    virtual int tpManager_GetMotEnables(int index, unsigned char *enables) = 0;

    virtual int tpManager_GetMotType(int index) = 0;

    // 0: double S/T = 0;  1: toppra
    virtual double tpManager_Getjerk() = 0;

    virtual int tpManager_Blend_next() = 0;
    virtual int tpManager_GetSafetyAcc(int index, double *safety_acc) = 0;
    virtual int tpManager_GetCurPlanVel(int index, double *currentVel) = 0;//获取规划主轴的current规划速度
    virtual int tpManager_GetStopDist(int index, double *stop_dist) = 0;

    // *****************************
    virtual void tpManager_tpSetCollision_rebound(int collision_rebound) = 0;
    virtual int tpManager_tpSetPos_JPos(ZucPose const* const pos, double* joint_pos) = 0;
    virtual int tpManager_tpRevertInterpolation() = 0;
    virtual int tpManager_tpTrajSmooth(int trqConActive, int firfltActive) = 0;
    virtual int tpManager_tpSetClsnOpt(CollisionOption clsnopt) = 0;
    virtual int tpManager_tpSetClsnStopAcc(ClsnStopAcc clsn_stop_acc) = 0;

    virtual void tpManager_tpSetPose(ZucPose const pose) = 0;
    virtual void tpManager_tpSetLastPose(ZucPose const pose) = 0;
    virtual void tpGetCurJointPos(double* jpos) = 0;
    virtual void tpManager_tpSetCurJpos(double* joint_pos) = 0;
    virtual void tpManager_tpSetFilterJpos(double* joint_pos) = 0;
    virtual void tpManager_tpGetGoalJointPos(double* jpos) = 0;
    virtual void tpManager_tpSetGoalPose(ZucPose const pos) = 0;
    virtual void tpManager_tpGetGoalPose(ZucPose* const pos) = 0;
    virtual void tpManager_tpGetlastPose(ZucPose* const pos) = 0;
    virtual void tpManager_tpGetlastJointPos(double* jpos) = 0;
    virtual void tpManager_tpSetLastJpos(double* jpos) = 0;
    virtual int tpManager_tpQueueDepth() = 0;
    virtual int tpManager_tpActiveDepth() = 0;
    virtual int tpManager_tpGetErrAddLine() = 0;
    virtual int tpManager_tpDepth() = 0;
    virtual int tpManager_tpGetExecId() = 0;

    virtual int tpManager_tcqFull() = 0;
    virtual bool tpManager_tcqEmpty() = 0;



    virtual double tpManager_calTimeTTraj(double posLen, double vmax, double amax) = 0;

    virtual int tpManager_VoilateLimit_Edge(int joint_num, int* pre_check_limit) = 0;

    virtual int tpManager_tpSetId(int id) = 0;
    virtual int tpManager_tpSetTermCond(int cond, double tolerance) = 0;

    // virtual int tpManager_tpSetVlimit(double vLimit) = 0;

    virtual void tpManager_tpSetmovsTimeSacle(double scale) = 0;

    // =================  IO
    // virtual int tpManager_tpSetDout(char ioType, int index, unsigned char start, unsigned char end) = 0;

    // virtual int tpManager_tpSetAout(char ioType, int index, unsigned char start, unsigned char end) = 0;

    virtual int tpManager_tpSetEndForceCond(int axis, EndForceCond endForceCond) = 0;

    // =================  Motion Cmd
    virtual int tpManager_tpAddJointLine(ZucPose end,
                                        int canon_motion_type,
                                        double vel,
                                        double ini_maxvel,
                                        double acc,
                                        double jerk,
                                        unsigned char enables,
                                        char atspeed,
                                        int indexrotary,
                                        EndCond endCond,
                                        int jointNum) = 0;

    virtual int tpManager_tpAddMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time) = 0;
    virtual int tpManager_tpAddLine(ZucPose end,
                                   int canon_motion_type,
                                   double vel,
                                   double ini_maxvel,
                                   double acc,
                                   double orivel,
                                   double oriacc,
                                   double jerk,
                                   unsigned char enables,
                                   char atspeed,
                                   int indexrotary,
                                   int using_abc_vel,
                                   EndCond endCond) = 0;

    virtual int tpManager_tpAddMovc(ZucPose mid,
                                   ZucPose end,
                                   double vel,
                                   double ini_maxvel,
                                   double acc,
                                   double jerk,
                                   unsigned char enables,
                                   int using_abc_vel,
                                   double circle_count,
                                   EndCond endCond,
                                   int circle_mode) = 0;

    virtual int tpManager_tpAddRigidTap(ZucPose end, double vel, double ini_maxvel, double acc, unsigned char enables) = 0;

    virtual int tpManager_tpAddCircle(ZucPose end,
                                     PmCartesian center,
                                     PmCartesian normal,
                                     int turn,
                                     int canon_motion_type,
                                     double vel,
                                     double ini_maxvel,
                                     double acc,
                                     double jerk,
                                     unsigned char enables,
                                     char atspeed) = 0;

    virtual int tpManager_tpAddLine_toppra(ZucPose end,
                                          int canon_motion_type,
                                          double vel,
                                          double ini_maxvel,
                                          double acc,
                                          double jerk,
                                          double ori_vel,
                                          double ori_acc,
                                          unsigned char enables,
                                          char atspeed,
                                          int indexrotary,
                                          int using_abc_vel,
                                          EndCond endCond,
                                          int already_inpos,
                                          int planner_type,
                                          int jpos_len,
                                          TcPathInfo* path_info,
                                          TcPathInfo* blend_path_info,
                                          const int cur_blend_left,
                                          const int cur_last_blend_len,
                                          const int blend_jpos_num,
                                          ConstraintsToppra* cons_toppra,
                                          const int id) = 0;

    virtual int tpManager_tpAddJointLine_toppra(ZucPose end,
                                               int canon_motion_type,
                                               double vel,
                                               double ini_maxvel,
                                               double acc,
                                               double jerk,
                                               unsigned char enables,
                                               char atspeed,
                                               int indexrotary,
                                               EndCond endCond,
                                               int jointNum,
                                               int already_inpos,
                                               int planner_type,
                                               int jpos_len,
                                               TcPathInfo* path_info,
                                               TcPathInfo* blend_path_info,
                                               const int cur_blend_left,
                                               const int cur_last_blend_len,
                                               const int blend_jpos_num,
                                               ConstraintsToppra* cons_toppra,
                                               const int id) = 0;

    virtual int tpManager_tpAddMovc_toppra(ZucPose mid,
                                          ZucPose end,
                                          double vel,
                                          double ini_maxvel,
                                          double acc,
                                          double jerk,
                                          unsigned char enables,
                                          int using_abc_vel,
                                          double circle_count,
                                          int circle_mode,
                                          EndCond endCond,
                                          int already_inpos,
                                          int planner_type,
                                          int jpos_len,
                                          TcPathInfo* path_info,
                                          TcPathInfo* blend_path_info,
                                          const int cur_blend_left,
                                          const int cur_last_blend_len,
                                          const int blend_jpos_num,
                                          ConstraintsToppra* cons_toppra,
                                          const int id) = 0;

    virtual int tpManager_tpSetTrackLinearCfgEna(unsigned char enable, double pulseEquivalent, PmCartesian conveyorDir) = 0;

    virtual int tpManager_tpSetTrackCircularCfgEna(unsigned char enable, double pulseEquivalent, PmHomogeneous o_to_world, int rotate_tool) = 0;

    // E-Stop
    virtual int tpManager_AddRecoverTc(const double* joint_fb_pos, ZucPose* cart_fb_pos, ZucPose* estop_cart_pos, const double* estop_joint_pos) = 0;
    virtual int tpManager_EstopRecoverUpdate() = 0;
    virtual void tpManager_tpGetEStopTcCurJointPos(double* jpos) = 0;

    virtual double tpManager_GetCurEStopProgress() = 0;
    virtual double tpManager_GetCurEStopTarget() = 0;
    virtual int tpManager_GetEStopPose(ZucPose* const pos) = 0;

    virtual void tpManager_InitRecoverTc() = 0;
    virtual void tpManager_updateTcDataForEStop() = 0;

    // track conveyor
    virtual void tpManager_GetConveyDir(PmCartesian* dir) = 0;
    virtual void tpManager_GetConveyO2world(PmHomogeneous* o2world) = 0;
    virtual int tpManager_GetConveyRotTool() = 0;
    virtual unsigned char tpManager_GetConveyEnabled() = 0;
    virtual char tpManager_GetConveyAnyChanged() = 0;
    virtual int tpManager_calculate_max_acc_stop_delta_progress(double current_vel, double max_acc, double* delta_target) = 0;

    virtual int tpManager_init_copy_kine(std::shared_ptr<kine::KineItf> kine) = 0;
    virtual int tpManager_init_copy_dyna(std::shared_ptr<::JAKADynIntf> dyna) = 0;

    virtual int tpManager_tpinit() = 0;
    virtual int tpManager_QueueDepth() = 0;
    virtual double tpManager_cycletime() = 0;
    virtual ZucPose tpManger_tpGetdtg() = 0; 
    virtual int tp_main_update() = 0;
};
std::shared_ptr<TP_Itf> create_tp();
}